#!/usr/bin/env python3
from flask import Flask, render_template, jsonify, request
from flask_socketio import SocketIO
from nav_controller import NavController
from ros_bridge import ROSBridge
import rclpy
import threading
import os
import yaml

"""处理HTTP和WebSocket请求"""
class NavWebServer:
    def __init__(self):
        self.app = Flask(__name__, template_folder='../templates', static_folder='../static')
        self.app.config['SECRET_KEY'] = 'secret_key'
        self.socketio = SocketIO(self.app, cors_allowed_origins="*")
        
        # 加载配置
        self.config = self.load_config()
        
        # ROS初始化
        self.ros_node = None
        self.nav_controller = None
        self.init_ros_thread()
        
        # 设置路由
        self.setup_routes()
        
        # 静态文件路由
        self.setup_static_routes()

    def load_config(self):
        config_path = os.path.join(
            os.path.dirname(__file__), '../config/settings.yaml')
        with open(config_path) as f:
            return yaml.safe_load(f)

    def init_ros_thread(self):
        def ros_thread():
            rclpy.init()
            self.ros_node = ROSBridge(self.socketio)
            self.nav_controller = NavController(self.ros_node)
            
            executor = rclpy.executors.MultiThreadedExecutor()
            executor.add_node(self.ros_node)
            
            try:
                executor.spin()
            finally:
                self.ros_node.destroy_node()
                rclpy.shutdown()
        
        threading.Thread(target=ros_thread, daemon=True).start()

    def setup_routes(self):
        @self.app.route('/')
        def index():
            return render_template('index.html', 
                                 map_config=self.config.get('map', {}))
        
        @self.app.route('/api/map')
        def get_map():
            if self.ros_node and self.ros_node.map_data:
                return self.ros_node.map_data, 200, {'Content-Type': 'image/png'}
            return jsonify({'error': 'Map not available'}), 404
        
        @self.app.route('/api/robot/status')
        def get_robot_status():
            if self.ros_node and self.ros_node.robot_pose:
                return jsonify(self.ros_node.robot_pose)
            return jsonify({'error': 'Robot pose not available'}), 404
        
        @self.app.route('/api/navigation/cancel', methods=['POST'])
        def cancel_navigation():
            if self.nav_controller:
                self.nav_controller.cancel_navigation()
                return jsonify({'status': 'success'})
            return jsonify({'error': 'Nav controller not ready'}), 503

    def setup_static_routes(self):
        @self.app.route('/static/<path:filename>')
        def static_files(filename):
            return send_from_directory('../static', filename)
        
        @self.app.route('/assets/<path:filename>')
        def asset_files(filename):
            return send_from_directory('../assets', filename)

    def setup_socket_handlers(self):
        @self.socketio.on('connect')
        def handle_connect():
            print('Client connected')
            if self.ros_node:
                # 发送当前状态给新连接的客户端
                if self.ros_node.map_info:
                    self.socketio.emit('map_update', {
                        'info': self.ros_node.map_info
                    })
                if self.ros_node.robot_pose:
                    self.socketio.emit('robot_pose', 
                                      self.ros_node.robot_pose)
                if self.ros_node.current_path:
                    self.socketio.emit('path_update', {
                        'path': self.ros_node.current_path
                    })

        @self.socketio.on('set_initial_pose')
        def handle_initial_pose(data):
            if self.nav_controller:
                self.nav_controller.set_initial_pose(
                    data['x'], data['y'], data['theta'])

        @self.socketio.on('navigate_to')
        def handle_navigation(data):
            if self.nav_controller:
                self.socketio.start_background_task(
                    self.nav_controller.navigate_to,
                    data['x'], data['y'], data['theta'])

        @self.socketio.on('set_waypoints')
        def handle_waypoints(data):
            if self.nav_controller:
                self.socketio.start_background_task(
                    self.nav_controller.follow_waypoints,
                    data['waypoints'])

        @self.socketio.on('velocity_command')
        def handle_velocity_command(data):
            if self.ros_node:
                self.ros_node.send_velocity_command(
                    data['linear'], data['angular'])

    def run(self, host=None, port=None):
        host = host or self.config.get('server', {}).get('host', '0.0.0.0')
        port = port or self.config.get('server', {}).get('port', 6002)
        self.socketio.run(self.app, host=host, port=port, debug=True)

if __name__ == '__main__':
    server = NavWebServer()
    server.setup_socket_handlers()
    server.run()